/*************************************************************************
*
*  sd84.c
*
*
*  Copyright 2008,2009 Steve Stancliff
*
*
*  Part of the TAOSF SSA system.
*
*
*  This module interfaces to the Devantech SD84 digital/analog/PWM i/o board
*
*
*  version 0.2, 1/8/09, SBS
*   - cleanup
*
*  version 0.1, 12/5/08, SBS
*   - first version based on compass.c
*
**************************************************************************
*
*  This program is free software; you can redistribute it and/or modify
*  it under the terms of the GNU General Public License as published by
*  the Free Software Foundation; either version 2 of the License, or
*  (at your option) any later version.
*
*  This program is distributed in the hope that it will be useful, but 
*  WITHOUT ANY WARRANTY; without even the implied warranty of 
*  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 
*  General Public License for more details.
*
*  You should have received a copy of the GNU General Public License 
*  along with this program; if not, write to the Free Software Foundation, 
*  Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA 
*
**************************************************************************/

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <unistd.h>

#include "logfile.h"
#include "delay.h"

#include "cssl.h"


static cssl_t *serial;

static int lastcommand;
static int lastargument;

enum cmds { 
	SET_SERVO=1, 
	GET_SERVO, 
	SET_SERVOSPEED, 
	SET_MODE, 
	GET_MODE, 
	SET_ANALOG_COUNT, 
	GET_ANALOG_COUNT, 
	GET_DIGITAL_INPUT, 
	GET_ANALOG_INPUT, 
	GET_VERSION,
	PAD1, 
	PAD2, 
	PAD3, 
	PAD4, 
	PAD5, 
	PAD6, 
	PAD7, 
	PAD8, 
	PAD9, 
	PAD10, 
	GET_BULK, 
	TEST_MODE, 
	MAX_CMDS
};


enum errors {
	OK=0, 
	WARNING, 
	RANGE_ERROR, 
	MODE_ERROR 
};


enum modes { 
	MIN_MODES=20, 
	MODE_OUTPUT_LOW, 
	MODE_OUTPUT_HIGH, 
	MODE_INPUT_DIGITAL, 
	MODE_INPUT_ANALOG, 
	MODE_SERVO, 
	MAX_MODES 
};


/********************************************************
* Relationship between analog channels and port numbers
*
* 1 21
* 2 22
* 3 20
* 4 18
* 5 28
* 6 27
* 7 29
* 8 26
* 9 30
* 10 11
* 11 10
* 12 9
* 13 7
* 14 38
* 15 37
* 16 39
* 17 36
* 18 40
* 19 84
* 20 83
* 21 82
* 22 80
* 23 48
* 24 47
* 25 49
* 26 46
* 27 50
* 28 73
* 29 72
* 30 71
* 31 69
* 32 58
* 33 57
* 34 59
* 35 56
* 36 60
*
* i.e., if you set the number of analog channels = 5,
*   the analog ports will be (21,22,20,18,28)
*
*****************************************************/



/*************************
*
* position : -100 = full left ; +100 = full right
*
*************************/
static void set_servo(int channel,int position)
{
	uint8_t buf[8];
	uint8_t *j;
	uint16_t value;

//	printf("percent = %d\n",percent);

//	value = 1000 + 10*percent;	
	value = 1500 + -5*position;	
//	printf("value = %d\n",value);
	
	j = (uint8_t *)&value;
	
//	printf("MSB:LSB = %d:%d\n"
//			,*j
//			,*(j+1));

//	printf("MSB:LSB = %x:%x\n"
//			,*j
//			,*(j+1));


	buf[0] = 0xaa;		
	buf[1] = 0xa0;
	buf[2] = 0x55;
	buf[3] = SET_SERVO;
	buf[4] = (uint8_t)channel;	
	buf[5] = 0x02;			
	buf[6] = *j;				// low byte
	buf[7] = *(j+1);			// high byte
	
	cssl_putdata(serial,buf,8);

	lastcommand = SET_SERVO;
	lastargument = channel;
	
	return;
}


/*************************
*
*
*************************/
static void get_servo(int channel)
{
	uint8_t buf[6];

	buf[0] = 0xaa;		
	buf[1] = 0xa0;
	buf[2] = 0x55;
	buf[3] = GET_SERVO;
	buf[4] = (uint8_t)channel;			
	buf[5] = 0;			

	cssl_putdata(serial,buf,6);

	lastcommand = GET_SERVO;
	lastargument = channel;
	
	return;
}


/*************************
*
*
*************************/
static void set_servo_speed(int channel,int speed)
{
	uint8_t buf[7];

	buf[0] = 0xaa;		
	buf[1] = 0xa0;
	buf[2] = 0x55;
	buf[3] = SET_SERVOSPEED;
	buf[4] = (uint8_t)channel;			
	buf[5] = 1;			
	buf[6] = (uint8_t)speed;			

	cssl_putdata(serial,buf,7);

	lastcommand = SET_SERVOSPEED;
	lastargument = channel;
	
	return;
}


/*************************
*
*
*************************/
static void set_mode(int channel,int mode)
{
	uint8_t buf[7];

	buf[0] = 0xaa;		
	buf[1] = 0xa0;
	buf[2] = 0x55;
	buf[3] = SET_MODE;
	buf[4] = (uint8_t)channel;
	buf[5] = 1;			
	buf[6] = (uint8_t)mode;			

	cssl_putdata(serial,buf,7);

	lastcommand = SET_MODE;
	lastargument = channel;

	return;
}


/*************************
*
*
*************************/
static void get_mode(int channel)
{
	uint8_t buf[6];

	buf[0] = 0xaa;		
	buf[1] = 0xa0;
	buf[2] = 0x55;
	buf[3] = GET_MODE;
	buf[4] = (uint8_t)channel;	
	buf[5] = 0;			

	cssl_putdata(serial,buf,6);

	lastcommand = GET_MODE;
	lastargument = channel;

	return;
}


/*************************
*
*
*************************/
static void set_analog_channels(int count)
{
	uint8_t buf[7];

	buf[0] = 0xaa;		
	buf[1] = 0xa0;
	buf[2] = 0x55;
	buf[3] = SET_ANALOG_COUNT;
	buf[4] = 0;	
	buf[5] = 1;			
	buf[6] = (uint8_t)count;	
	
	cssl_putdata(serial,buf,7);

	lastcommand = SET_ANALOG_COUNT;

	return;
}


/*************************
*
*
*************************/
static void get_analog_channels(void)
{
	uint8_t buf[6];

	buf[0] = 0xaa;		
	buf[1] = 0xa0;
	buf[2] = 0x55;
	buf[3] = GET_ANALOG_COUNT;
	buf[4] = 0;	
	buf[5] = 0;			

	cssl_putdata(serial,buf,6);

	lastcommand = GET_ANALOG_COUNT;

	return;
}


/*************************
*
*
*************************/
static void get_input_digital(int channel)
{
	uint8_t buf[6];

	buf[0] = 0xaa;		
	buf[1] = 0xa0;
	buf[2] = 0x55;
	buf[3] = GET_DIGITAL_INPUT;
	buf[4] = (uint8_t)channel;
	buf[5] = 0;			

	cssl_putdata(serial,buf,6);

	lastcommand = GET_DIGITAL_INPUT;
	lastargument = channel;

	return;
}


/*************************
*
*
*************************/
static void get_input_analog(int channel)
{
	uint8_t buf[6];

	buf[0] = 0xaa;		
	buf[1] = 0xa0;
	buf[2] = 0x55;
	buf[3] = GET_ANALOG_INPUT;
	buf[4] = (uint8_t)channel;
	buf[5] = 0;			

	cssl_putdata(serial,buf,6);

	lastcommand = GET_ANALOG_INPUT;
	lastargument = channel;

	return;
}


/*************************
*
*
*************************/
static void get_version(void)
{
	uint8_t buf[6];

	buf[0] = 0xaa;		
	buf[1] = 0xa0;
	buf[2] = 0x55;
	buf[3] = GET_VERSION;
	buf[4] = 2;			// CPU number
	buf[5] = 0;			

	cssl_putdata(serial,buf,6);

	lastcommand = GET_VERSION;

	return;
}


/*************************
*
*
*************************/
static void get_bulk(int cpu)
{
	uint8_t buf[6];

	if(cpu<1 || cpu>4){
		#ifdef DEBUG
		logfile_LogPrint("get_bulk() - invalid CPU number");
		#endif
		printf("get_bulk() - invalid CPU number\n");
		return;
	}
	
	buf[0] = 0xaa;		
	buf[1] = 0xa0;
	buf[2] = 0x55;
	buf[3] = GET_BULK;
	buf[4] = (uint8_t)cpu;			
	buf[5] = 0;			

	cssl_putdata(serial,buf,6);

	lastcommand = GET_BULK;
	lastargument = cpu;

	return;
}


/*************************
*
*
*************************/
void InterruptCallback(int signum)
{
	#ifdef DEBUG
	char str[LOGSTRLEN];

	snprintf(str,LOGSTRLEN, "\nExiting on signal %d!", signum);
	logfile_LogPrint(str);
	logfile_CloseFile();
	#endif

	printf("\nExiting on signal %d!\n", signum);
	cssl_close(serial);
	cssl_stop();
	exit(signum);
}


/*************************
*
*
*************************/
static void SetIntCallback()
{
	signal(SIGINT, InterruptCallback);
	signal(SIGTERM, InterruptCallback);
	signal(SIGQUIT, InterruptCallback);
	signal(SIGHUP, InterruptCallback);
	
	return;
}

/*************************
*
*
*************************/
void sd84_parseresponse(
			uint8_t *buf,
			int length)
{
	switch(lastcommand){
		case GET_MODE:
		
			printf("pin #%d  ",lastargument);
			
			switch(buf[0]){
				case	MODE_OUTPUT_LOW:
					printf("sd84_parseresponse(): mode = output low\n");
					break;
				case MODE_OUTPUT_HIGH:
					printf("mode = output high\n");
					break;
				case MODE_INPUT_DIGITAL:
					printf("mode = input digital\n");
					break;
				case MODE_INPUT_ANALOG:
					printf("mode = input analog\n");
					break;
				case MODE_SERVO:
					printf("mode = servo\n");
					break;
			}
			break;
			
		case GET_DIGITAL_INPUT:
			if(length==1){
				printf("pin #%d  state = %d\n",lastargument,buf[0]);
			}
			break;
			
		case GET_VERSION:
			if(length==2){
				printf("board #%d  revision #%d\n",buf[0],buf[1]);
			}
			break;

		case SET_SERVO:	
			if(length==1){
				printf("set servo #%d  rval = %d\n",lastargument,buf[0]);
			}else{
				printf("'set servo' wrong response length\n");
			}
			break;

		case GET_SERVO:	
			if(length==2){
				int i;
				uint8_t *j;

				j=(uint8_t *)&i;

				*j = buf[0];
				*(j+1) = buf[1];
				
				printf("get servo #%d  value = %d\n",lastargument,i);
			}else{
				printf("'get servo' wrong response length\n");
			}
			break;

		case SET_MODE:	
			if(length==1){
				printf("set mode #%d  rval = %d\n",lastargument,buf[0]);
			}
			break;

	}
	
	return;
}


/*************************
*
*
*************************/
void sd84_callback(
			int id,
			uint8_t *buf,
			int length)
{
	register int i;

	printf("\n");
	for(i=0;i<length;i++){
		printf("%d",buf[i]);
		if(i+1<length){
			printf(":");
		}
	}
	printf("\n");
	
	sd84_parseresponse(buf,length);

	return;
}


/*************************
*
*
*************************/
static void OpenLogfile(
			char *logfilename)
{
	#ifdef DEBUG
	logfile_OpenFile(logfilename);
	logfile_LogPrint(" ");
	logfile_LogPrint("*******************************************");		
	logfile_LogPrint("SD84 STARTUP");
	logfile_LogPrint("*******************************************");		
	logfile_LogPrint(" ");
	#endif

	return;
}


/*************************
*
*
*************************/
static int sd84Init(
			char *port
			,int baud
			,int bits
			,int stopbits)
{
	cssl_start();
    
	serial = cssl_open(
				port
				,sd84_callback
				,0
				,baud
				,bits
				,0
				,1);
	
	if (!serial) {
		printf("%s\n",cssl_geterrormsg());
		return(-1);
	}

	return(0);
}


/*************************
*
*
*************************/
static void mainloop()
{	
	TIMESPEC delay;
//	char	logstr[LOGSTRLEN];
	int i;
	
	
	delay = delay_setdelay(2.0);
	
	
	i=0;
		
		
	while(1){
		switch(i){
			case 0:
				get_version();
				break;
			case 1:
				get_mode(2);
				break;
			case 2:
				set_mode(2,MODE_SERVO);
				break;
			case 3:
				get_mode(2);
				break;
			case 4:
				set_servo(2,-100);
				break;
			case 5:
				get_servo(2);
				break;
			case 6:
				get_input_digital(12);
				break;
		}
		i++;
		
		printf(".");
		fflush(stdout);
		
		delay_sleep(&delay);
	}	
	
	return;
}


/*************************
*
*
*************************/
int main(int argc, char **argv)
{
	char *port = "/dev/sd84_0";
	int baud = 115200;
	int bits = 8;
	int stopbits = 2;
	
	OpenLogfile("logfile.log");
		
	SetIntCallback();

	sd84Init(port,baud,bits,stopbits);
	
	mainloop();

	return(0);
}

